#include "movingobject.h"

MovingObject::MovingObject():Object()
{
          m_maxSpeed = 5;
          m_maxTurnRate =0.5;
          m_velocity=QVector2D(1,0.5);
          m_heading=QVector2D(0,1);
}

const QVector2D&  MovingObject::velocity()const
{
    return m_velocity;
}

void  MovingObject::setVelocity(const QVector2D& v)
{
    m_velocity=v;
}

const QVector2D&   MovingObject::heading()const
{
    return m_heading;
}
void  MovingObject::setHeading(const QVector2D& heading)
{
    m_heading=heading; 
}

bool  MovingObject::rotateToTarget(const QVector2D& target)
{

    QVector2D toTarget = (target-QVector2D(this->scenePos())).normalized();

    double angle = acos(QVector2D::dotProduct(m_heading,toTarget));

    if(angle < 0.000001) return true;
    if (angle > m_maxTurnRate) angle = m_maxTurnRate;

    //wybieram kierunek obrotu

    angle*=(m_heading.y()*toTarget.x()<=m_heading.x()*toTarget.y())?1:-1;
    //this->setRotation(rotation()+angle);
    m_heading=rotate(m_heading,angle);
    m_velocity=rotate(m_velocity,angle);

    return false;
}
QVector2D MovingObject::rotate(const QVector2D& v, double angle)const
{
    double sin=std::sin(angle);
    double cos=std::cos(angle);
    QMatrix rotation(cos,sin,-sin,cos,0,0);
    QPointF p=v.toPointF()*rotation;
    QVector2D x(p.x(),p.y());
    return x;
}


inline double    MovingObject::maxSpeed()const
{
    return m_maxSpeed;
}
void      MovingObject::setMaxSpeed(double ms)
{
    m_maxSpeed=ms;
}

inline bool      MovingObject::isSpeedMaxedOut()const
{
    return  m_velocity.lengthSquared()<= m_maxSpeed*m_maxSpeed;

}
inline double    MovingObject::speed()const
{
    return m_velocity.length();
}
inline double    MovingObject::speedSquared()const
{
    return m_velocity.lengthSquared();
}
inline double    MovingObject::maxTurnRate()const
{
    return m_maxTurnRate;
}
void      MovingObject::setMaxTurnRate(double mtr)
{
    m_maxTurnRate=mtr;
}
